//
//  Transform.hpp
//  RayTracer
//
//  Created by 俞云康 on 2/22/16.
//  Copyright © 2016 yushroom. All rights reserved.
//

#ifndef Transform_hpp
#define Transform_hpp

#include "Math.hpp"
#include "Ray.hpp"

class Params;

struct Matrix4x4 {
    // Matrix4x4 Public Methods
    float m[4][4];
    
    Matrix4x4() {
        memset(m, 0, sizeof(*this));
        m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.f;
    }
	Matrix4x4(const Matrix4x4& mat);
    Matrix4x4(float mat[4][4]);
    Matrix4x4(float t00, float t01, float t02, float t03,
              float t10, float t11, float t12, float t13,
              float t20, float t21, float t22, float t23,
              float t30, float t31, float t32, float t33);
    bool operator==(const Matrix4x4 &m2) const {
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j)
                if (m[i][j] != m2.m[i][j]) return false;
        return true;
    }
    bool operator!=(const Matrix4x4 &m2) const {
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j)
                if (m[i][j] != m2.m[i][j]) return true;
        return false;
    }
    friend Direction operator*(const Direction& dir, const Matrix4x4& m) {
        float3 v;
        v.x = dir.x*m.m[0][0] + dir.y*m.m[1][0] + dir.z*m.m[2][0];
        v.y = dir.x*m.m[0][1] + dir.y*m.m[1][1] + dir.z*m.m[2][1];
        v.z = dir.x*m.m[0][2] + dir.y*m.m[1][2] + dir.z*m.m[2][2];
        return v;
    }
    friend float4 operator*(const float4& v4, const Matrix4x4& m) {
        float4 v;
        for (int i = 0; i < 4; ++i) {
            v[i] = v4.x*m.m[0][i] + v4.y*m.m[1][i] + v4.z*m.m[2][i] + v4.w*m.m[3][i];
        }
        return v;
    }
    Matrix4x4 operator*(const Matrix4x4& m2) const {
        Matrix4x4 r;
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j)
                r.m[i][j] = m[i][0] * m2.m[0][j] + m[i][1] * m2.m[1][j] +
                m[i][2] * m2.m[2][j] + m[i][3] * m2.m[3][j];
        return r;
    }
    
    bool is_identity() const {
        return (is_one(m[0][0]) && m[0][1] == 0.f && m[0][2] == 0.f && m[0][3] == 0.f &&
                m[1][0] == 0.f && is_one(m[1][1]) && m[1][2] == 0.f && m[1][3] == 0.f &&
                m[2][0] == 0.f && m[2][1] == 0.f && is_one(m[2][2]) && m[2][3] == 0.f &&
                m[3][0] == 0.f && m[3][1] == 0.f && m[3][2] == 0.f && is_one(m[3][3]));
    }
    
    friend Matrix4x4 transpose(const Matrix4x4 &);
    friend Matrix4x4 inverse(const Matrix4x4 &);
};


class Transform {
private:
    Matrix4x4 m;        // local to world
    Matrix4x4 m_inv;    // world to local
    
public:
    Transform() = default;
    Transform(const Matrix4x4& m, const Matrix4x4& m_inv) : m(m), m_inv(m_inv) {
        Assert((m*m_inv).is_identity());
    }
    Transform(const Matrix4x4& m) : m(m), m_inv(inverse(m)) { }
    
    static std::shared_ptr<Transform> create(const Params& params);
    
    Position point2world(const Position& p) const {
        return (float4(p, 1) * m).as_point();
    }
    
    Direction direction2world(const Direction& p) const {
        return p * m;
    }
    
    Normal normal2world(const Normal& n) const {
		//TODO
        //return n * m;
        return Normal(n.x*m_inv.m[0][0] + n.y*m_inv.m[0][1] + n.z*m_inv.m[0][2],
                      n.x*m_inv.m[1][0] + n.y*m_inv.m[1][1] + n.z*m_inv.m[1][2],
                      n.x*m_inv.m[2][0] + n.y*m_inv.m[2][1] + n.z*m_inv.m[2][2]);
    }
    
    Ray Ray2world(const Ray& r) const {
        Ray ret = r;
        ret.o = point2world(ret.o);
        ret.dir = direction2world(ret.dir);
        return ret;
    }
    
    Position point2local(const Position& p) const {
        return (float4(p, 1) * m_inv).as_point();
    }
    
    Direction direction2local(const Direction& p) const {
        return p * m_inv;
    }
    
    Normal normal2local(const Normal& n) const {
		//TODO
        return n * m_inv;
    }
    
    Ray Ray2local(const Ray& r) const {
        Ray ret = r;
        ret.o = point2local(ret.o);
        ret.dir = direction2local(ret.dir);
        return ret;
    }

	bool isIdentity() const {
		return m.is_identity();
	}
};


static Matrix4x4 rotation_around(const Direction& axis, const float sin_theta, const float cos_theta) {
    Direction n = Normalize(axis);
    float c = cos_theta;
    float one_c = 1-c;
    float xx = n.x*n.x;
    float xy = n.x*n.y;
    float xz = n.x*n.z;
    float yz = n.y*n.z;
    float yy = n.y*n.y;
    float zz = n.z*n.z;
    float sx = sin_theta*n.x;
    float sy = sin_theta*n.y;
    float sz = sin_theta*n.z;
    return Matrix4x4(c+one_c*xx,  one_c*xy+sz, one_c*xz-sy, 0,
                     one_c*xy-sz, c+one_c*yy,  one_c*yz+sx, 0,
                     one_c*xz+sy, one_c*yz-sx, c+one_c*zz,  0,
                     0,           0,           0,           1);
}

inline Matrix4x4 rotation_around(const Direction& axis, const float radian) {
    return rotation_around(axis, sinf(radian), cosf(radian));
}

static Matrix4x4 rotation_from_to(const Direction& from, const Direction& to) {
    Direction a = Normalize(from);
    Direction b = Normalize(to);
    // nearly the same direction
    if (fabsf(Dot(a, b)-1) < EPS) {
        return Matrix4x4();
    }
    Direction axis;
    // nearly the opposite direction
    if (fabsf(Dot(a, b)+1) < EPS) {
        if (a.x >= a.y && a.x >= a.z) {
            axis = float3(a.z, 0, -a.x);
        } else if (a.y >= a.z) {
            axis = float3(a.y, -a.x, 0);
        } else {
            axis = float3(0, a.z, -a.y);
        }
    }
    else {
        axis = Cross(from, to);
    }
    return rotation_around(axis, axis.length(), Dot(a, b));
}

inline Matrix4x4 translation(float x, float y, float z) {
    return Matrix4x4(1, 0, 0, 0,
                     0, 1, 0, 0,
                     0, 0, 1, 0,
                     x, y, z, 1);
}

inline Matrix4x4 translation(const Position& p) {
    return translation(p.x, p.y, p.z);
}

inline Matrix4x4 scale(float s) {
    return Matrix4x4(s, 0, 0, 0,
                     0, s, 0, 0,
                     0, 0, s, 0,
                     0, 0, 0, 1);
}

inline Matrix4x4 scale(float sx, float sy, float sz) {
    return Matrix4x4(sx, 0,  0,  0,
                     0,  sy, 0,  0,
                     0,  0,  sz, 0,
                     0,  0,  0,  1);
}

static Matrix4x4 LookAt(const Position& eye, const Position& at, const Direction& up)
{
    float3 Z = Normalize(at - eye);         // z
    float3 X = Normalize(Cross(up, Z));     // x
    float3 Y = Normalize(Cross(Z, X));      // y
    return Matrix4x4(X.x, Y.x, Z.x, 0,
                     X.y, Y.y, Z.y, 0,
                     X.z, Y.z, Z.z, 0,
                     -Dot(X, eye), -Dot(Y, eye), -Dot(Z, eye), 1);
}


#endif /* Transform_hpp */
